/*******************************************************************************
* This file declares all the variables in flash to store the configuration of
* the system and also provides functions to access them.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include "flash.h"
	


/*******************************************************************************
* FLASH VARIABLES                                                              *
*******************************************************************************/

// The system configuration that saved in the flash.
static struct SYSTEM_CONFIG __attribute__((space(prog), aligned(_FLASH_PAGE * 2))) flash_xSystemConfig = {
	// Sensors calibration data.
	0,				// Gyro X value for 0 rad/s.
	PI / 2480.0f,	// Scale to change gyro value to rad/s.
	
	0,				// Gyro Y value for 0 rad/s.
	PI / 2480.0f,	// Scale to change gyro value to rad/s.
	
	0,				// Gyro Z value for 0 rad/s.
	PI / 2520.0f,	// Scale to change gyro value to rad/s.
	
	
	
	-2,				// Acc X value for 0G.
	2.0 / 522.0,	// Scale to change the value to G.
	
	6,				// Acc Y value for 0G.
	2.0 / 527.0,	// Scale to change the value to G.
	
	60,				// Acc Z value for 0G.
	2.0 / 503.0,	// Scale to change the value to G.
	
	
	
	-58,			// Mag X value for 0 gauss.
	1.937985e-3f,	// Scale to change the value to +-1 for Earth magnetic field.
	
	104,			// Mag Y value for 0 gauss.
	2.212389e-3f,	// Scale to change the value to +-1 for Earth magnetic field.
	
	101,			// Mag Z value for 0 gauss.
	1.976285e-3f,	// Scale to change the value to +-1 for Earth magnetic field.
	
	
	
//	// RC Rx calibration data.
//	7425,			// Mid value for RC Rx roll channel.
//	1.0 / 2500.0,	// Scale to change the value to +-1.
//	
//	7420,			// Mid value for RC Rx pitch channel.
//	1.0 / 2500.0,	// Scale to change the value to +-1.
//	
//	7420,			// Mid value for RC Rx yaw channel.
//	1.0 / 2500.5,	// Scale to change the value to +-1.
//	
//	4898,			// Minimum value for RC Rx throttle channel.
//	1.0 / 5000.0,	// Scale to change the value to 0 - 1.
	
	
	
	// The gain for the control system.
	1.5f,			// Roll Kp.
	0.2f,			// Roll Kd.
	1.0f,			// Roll Ki.
	
	1.5f,			// Pitch Kp.
	0.2f,			// Pitch Kd.
	1.0f,			// Pitch Ki.
	
	1.0f,			// Navigation Kp.
	0.005f,			// Navigation Ki.
	
	0.02f,			// Cross Track Kp.
	
	0.03f,			// Airspeed Kp.
	0.0015f,		// Airspeed Ki.
	
	1.0f			// Aileron - Rudder mix ratio.
};	

// We need to make sure the data size is smaller than a row.
// We will declare a dummy array for this checking.
int iDummyArray[(sizeof(struct SYSTEM_CONFIG) <= (_FLASH_ROW * 2)) - 1];



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

static char pcUartWriteBuffer[200];



/*******************************************************************************
* PUBLIC FUNCTION: vLoadFlashData
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Read all the value from flash and save them in xSystemConfig.
*
*******************************************************************************/
void vLoadFlashData(void)
{
	// Address in flash.
	_prog_addressT xFlashAddress;
	
	
	
	// Get the address of the system configuration in flash.
	_init_prog_address(xFlashAddress, flash_xSystemConfig);
	
	// Read the data from the flash.
	_memcpy_p2d16((char *)&xSystemConfig, xFlashAddress, sizeof(struct SYSTEM_CONFIG));
}	



/*******************************************************************************
* PUBLIC FUNCTION: vSaveFlashData
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Save all the value from xSystemConfig into the flash.
*
*******************************************************************************/
void vSaveFlashData(void)
{
	// Address in flash.
	_prog_addressT xFlashAddress;
	
	// Buffer for writing to flash.
	int iWriteBuffer[_FLASH_ROW];
	
	
	
	// Fill up the write buffer with the data we want to write.
	memcpy((void *)iWriteBuffer, (void *)&xSystemConfig, sizeof(xSystemConfig));
	
	// Fill the rest of the buffer with 0xffff.
	memset((void *)iWriteBuffer + sizeof(xSystemConfig), 0xffff, sizeof(iWriteBuffer) - sizeof(xSystemConfig));
	
	
	
	// Get the address of the system configuration in flash.
	_init_prog_address(xFlashAddress, flash_xSystemConfig);
	
	// Erase and write to the flash.
	_erase_flash(xFlashAddress);
	_write_flash16(xFlashAddress, iWriteBuffer);
}



/*******************************************************************************
* PUBLIC FUNCTION: vPrintFlashData
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Print out the value for each member of system confir.
*
*******************************************************************************/
void vPrintFlashData(void)
{
	sprintf(pcUartWriteBuffer, "Gyro X Offset: %d\r\n"
							   "Gyro X Scale: %e\r\n\r\n"
							   "Gyro Y Offset: %d\r\n"
							   "Gyro Y Scale: %e\r\n\r\n"
							   "Gyro Z Offset: %d\r\n"
							   "Gyro Z Scale: %e\r\n\r\n",
							   xSystemConfig.siGyroXMid,
							   (double)xSystemConfig.fGyroXScale,
							   xSystemConfig.siGyroYMid,
							   (double)xSystemConfig.fGyroYScale,
							   xSystemConfig.siGyroZMid,
							   (double)xSystemConfig.fGyroZScale);
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 100ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 10);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
	
	
	
	sprintf(pcUartWriteBuffer, "Acc X Offset: %d\r\n"
							   "Acc X Scale: %e\r\n\r\n"
							   "Acc Y Offset: %d\r\n"
							   "Acc Y Scale: %e\r\n\r\n"
							   "Acc Z Offset: %d\r\n"
							   "Acc Z Scale: %e\r\n\r\n",
							   xSystemConfig.siAccXMid,
							   (double)xSystemConfig.fAccXScale,
							   xSystemConfig.siAccYMid,
							   (double)xSystemConfig.fAccYScale,
							   xSystemConfig.siAccZMid,
							   (double)xSystemConfig.fAccZScale);
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 100ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 10);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
	
	
	
	sprintf(pcUartWriteBuffer, "Mag X Offset: %d\r\n"
							   "Mag X Scale: %e\r\n\r\n"
							   "Mag Y Offset: %d\r\n"
							   "Mag Y Scale: %e\r\n\r\n"
							   "Mag Z Offset: %d\r\n"
							   "Mag Z Scale: %e\r\n\r\n",
							   xSystemConfig.siMagXMid,
							   (double)xSystemConfig.fMagXScale,
							   xSystemConfig.siMagYMid,
							   (double)xSystemConfig.fMagYScale,
							   xSystemConfig.siMagZMid,
							   (double)xSystemConfig.fMagZScale);
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 100ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 10);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
	
	
	
	sprintf(pcUartWriteBuffer, "Roll Gain: %e\r\n"
							   "Pitch Gain: %e\r\n\r\n",
							   (double)xSystemConfig.fRollKp,
							   (double)xSystemConfig.fPitchKp);
	while (ucGetSendToPcSpace() < strlen(pcUartWriteBuffer)) {
		// If we are calling from task, delay for 100ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 10);
		}	
	}	
	ucSendToPc(pcUartWriteBuffer, strlen(pcUartWriteBuffer));
}	
